/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "movemagneticinstructionlogic.h"

#include "programset.h"
#include "mccontrollerthread.h"
#include "programengine.h"
#include "messagelog.h"

MoveMagneticInstructionLogic::MoveMagneticInstructionLogic(int robotIdIn, ProgramSet *programSetIn, ProgramEngine* programEngineIn,
                           McControllerThread *programMcControllerIn)
{
    robotId=robotIdIn;
    programSet=programSetIn;
    programEngine=programEngineIn;
    programMcController=programMcControllerIn;

}

E_PROGRAM_LOOP_RESULT MoveMagneticInstructionLogic::startMoveMagnetic(int programNumIn,
                                                                      MoveMagInstruction instructionIn)
{


    E_PROGRAM_LOOP_RESULT tmpResult=moveMagnetic_run(instructionIn);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpResult)
    {
        return tmpResult;
    }

    programEngine->calculateNextLineToRun(programNumIn);

    return programEngine->isTheLastInstruction(programNumIn);


}

E_PROGRAM_LOOP_RESULT MoveMagneticInstructionLogic::moveMagnetic_run(const MoveMagInstruction instructionIn)
{
    E_TRACK_STATUS tmpStatus;
    programMcController->mcControllerInterpreter->getTrackStatus(tmpStatus);
    if(E_TRACK_STATUS_PASSOVER!=tmpStatus)
    {
        programMcController->mcControllerInterpreter->resetVehicleMagneticMove();
        programMcController->mcControllerInterpreter->setMotionServerModel
                                             (ENUM_MOTION_MODEL_VEHICLE_MAGANIC_TRACK);

    }
    E_TRACK_STOP_CONDITION condition;
    bool tmpIsPassover;
    if( E_MOVE_MAG_NO_STOP==instructionIn.stopType)
    {
        condition=E_TRACK_STOP_CONDITION_NONE;
        tmpIsPassover=true;
    }
    else if( E_MOVE_MAG_RFID_STOP==instructionIn.stopType)
    {
        condition=E_TRACK_STOP_CONDITION_RFID;
        tmpIsPassover=false;
    }
    else if( E_MOVE_MAG_MAG_WIDTH_STOP==instructionIn.stopType)
    {
        condition=E_TRACK_STOP_CONDITION_MAGANETIC_WIDTH;
        tmpIsPassover=false;
    }
    else if( E_MOVE_MAG_INPUT_STOP==instructionIn.stopType)
    {
        condition=E_TRACK_STOP_CONDITION_INPUT;
        tmpIsPassover=false;
        programMcController->mcControllerInterpreter->setMaganeticInputIndex(
                    instructionIn.stopInputNum-1);
    }
    programMcController->mcControllerInterpreter->setTrackStopCondition(condition);


    double speedRatio=instructionIn.velocityRatio/100.0;
    if(E_MOVE_MAG_FRONT_DIRECTION==instructionIn.moveMagDirectionFB)
    {

    }
    else
    {
        speedRatio=-1*speedRatio;
    }
    if(E_MOVE_MAG_LEFT_DIRECTION==instructionIn.moveMagDirectionLMR)
    {
        programMcController->mcControllerInterpreter->setTrackMode(E_TRACK_MODE_MAGENETIC_LEFT);
    }
    else if(E_MOVE_MAG_MIDDLE_DIRECTION==instructionIn.moveMagDirectionLMR)
    {
        programMcController->mcControllerInterpreter->setTrackMode(E_TRACK_MODE_MAGENETIC_MIDDLE);
    }
    else if(E_MOVE_MAG_RIGHT_DIRECTION==instructionIn.moveMagDirectionLMR)
    {
        programMcController->mcControllerInterpreter->setTrackMode(E_TRACK_MODE_MAGENETIC_RIGHT);
    }

    programMcController->mcControllerInterpreter->setVehicleIdealSpeed_ratio(speedRatio);
    programMcController->mcControllerInterpreter->setMageneticPassRfid(instructionIn.
                                                       rfidInternalValue.toLongLong());

    programMcController->mcControllerInterpreter->startVehicleMagneticMove();



    //完成判断，没有则阻塞等待
    int isFinished=-1;
    while(-1==isFinished)
    {
        programMcController->mcControllerInterpreter->getTrackStatus(tmpStatus);
        if(true==tmpIsPassover && E_TRACK_STATUS_PASSOVER==tmpStatus )
        {
            isFinished=1;
        }
        if(false==tmpIsPassover && E_TRACK_STATUS_GOAL_REACHED==tmpStatus)
        {
            isFinished=1;
        }
        else if(E_TRACK_STATUS_ERROR==tmpStatus)
        {
            qDebug()<<"getTrackStatus==E_TRACK_STATUS_ERROR";
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3050, robotId
                    , programEngine->getRunningLineFloat() );
        }
        if(-1==isFinished)
        {
            usleep(D_MOTION_INSTRUCTION_WAIT_SLEEP);
        }

        if(ENUM_COMMAND_CEASE == programEngine->getEngineStatue() ||
                ENUM_COMMAND_RESET == programEngine->getEngineStatue())
        {
            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();

           return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }
        else if(ENUM_TRAJECTORY_STOPPED==programMcController->mcControllerInterpreter->getStatus()
                || ENUM_TRAJECTORY_CEASED==programMcController->mcControllerInterpreter->getStatus())
        {
            if(ENUM_COMMAND_CEASE != programEngine->getEngineStatue() &&
                    ENUM_COMMAND_RESET != programEngine->getEngineStatue())
            {
                qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3050, robotId
                        , programEngine->getRunningLineFloat() );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }
    }
    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT MoveMagneticInstructionLogic::moveMagnetic_run(const MaganeticPathParameter paraIn)
{
    MoveMagInstruction instruction;
    instruction.instructionType=MOVE_MAG;
    instruction.moveMagDirectionLMR=paraIn.moveMagDirectionLMR;
    instruction.moveMagDirectionFB=paraIn.moveMagDirectionFB;
    instruction.velocityRatio=paraIn.velocityRatio;
    instruction.rfidInternalValue=paraIn.rfidInternalValue;
    instruction.stopType=paraIn.stopType;
    instruction.stopInputNum=paraIn.stopInputNum;

    return moveMagnetic_run(instruction);
}

void MoveMagneticInstructionLogic::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotId, int parameter1, int parameter2, int parameter3, int parameter4, QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;


    QString infomationStr;

    switch(messageCode)
    {

    case 3051:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常（算法模块报错）！")+message;

        break;
    }

    case 3050:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常（算法模块进入停止状态）！")+message;

        break;
    }


    default:
    {

    }
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);

}
